import ogre.renderer.OGRE as ogre
from ogre.physics.bullet import *
from ogre.physics.OgreBulletC import *

def BulletTmToVtkTm(bttransform, scaling = 1.0):
    Transform = vtkMatrix4x4()
    Transform.Identity()

    for c in range(3):
        Transform.SetElement(0, c, bttransform.getBasis().getColumn(c).x())
        Transform.SetElement(1, c, bttransform.getBasis().getColumn(c).y())
        Transform.SetElement(2, c, bttransform.getBasis().getColumn(c).z())

    Transform.SetElement(0, 3, scaling * bttransform.getOrigin().x())
    Transform.SetElement(1, 3, scaling * bttransform.getOrigin().y())
    Transform.SetElement(2, 3, scaling * bttransform.getOrigin().z())

    return Transform

def VtkTmToBulletTm(vtktransform, scaling = 1.0):
    Transform = btTransform()

    Transform.setIdentity()

    Basis = btMatrix3x3()

    Basis.setValue(
        vtktransform.GetElement(0, 0),
        vtktransform.GetElement(0, 1),
        vtktransform.GetElement(0, 2),

        vtktransform.GetElement(1, 0),
        vtktransform.GetElement(1, 1),
        vtktransform.GetElement(1, 2),

        vtktransform.GetElement(2, 0),
        vtktransform.GetElement(2, 1),
        vtktransform.GetElement(2, 2)
    )

    Transform.setBasis(Basis)
    Transform.setOrigin(btVector3(scaling * vtktransform.GetElement(0, 3), scaling * vtktransform.GetElement(1, 3), scaling * vtktransform.GetElement(2, 3)))

    return Transform

class CollisionWorld():
    def __init__(self):
        self.BtCollisionConfiguration   = btDefaultCollisionConfiguration()
        self.BtDispatcher               = btCollisionDispatcher (self.BtCollisionConfiguration)
        self.BtBroadphase               = btDbvtBroadphase()
        self.BtWorld                    = btCollisionWorld(self.BtDispatcher, self.BtBroadphase, self.BtCollisionConfiguration)

class DynamicsWorld():
    def __init__(self):
        self.BtCollisionConfiguration   = btDefaultCollisionConfiguration()
        self.BtDispatcher               = btCollisionDispatcher (self.BtCollisionConfiguration)
        self.BtBroadphase               = btDbvtBroadphase()
        self.BtSolver                   = btSequentialImpulseConstraintSolver()
        self.BtWorld                    = btDiscreteDynamicsWorld(self.BtDispatcher, self.BtBroadphase , self.BtSolver, self.BtCollisionConfiguration)

class BoxRigidBody():
    def __init__(self):
        self.BoxShape     = btBoxShape(btVector3(1, 1, 1))
        self.Transform    = btTransform()
        self.Transform.setIdentity()

        self.InstrumentGeometry.BtCompoundShape.addChildShape(self.BtTransform, self.BtBoxShape)